from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    """
    一键启动视觉项目进行演示的核心launch文件。
    它会启动所有必要的节点，并打开一个标注图像的显示窗口。
    """

    usb_cam_params_file = os.path.join(get_package_share_directory("my_vision_pkg"),
                                       'config', 'usb_params.yaml')
    return LaunchDescription([
        # 1. 启动USB相机节点
        #    - 对应指令: ros2 run usb_cam usb_cam_node
        #    - 注意：有些usb_cam版本可执行文件名是 usb_cam_node_exe
        Node(
            package='usb_cam',
            executable='usb_cam_node_exe', # 如果版本不是这个，请改成 'usb_cam_node'
            name='usb_cam',
            parameters=[usb_cam_params_file]
        ),

        # 2. 启动核心的Python视觉节点
        #    - 对应指令: ros2 run my_vision_pkg vision_node
        Node(
            package='my_vision_pkg',
            executable='vision_node',
            name='vision_node'
        ),

        # 3. 启动TF广播节点 (用于RViz可视化)
        #    - 对应指令: ros2 run my_vision_pkg tf_broadcaster_node
        Node(
            package='my_vision_pkg',
            executable='tf_broadcaster_node',
            name='tf_broadcaster_node'
        ),
        
        # 4. 启动编写的CAN控制器节点
        #    - 这个节点负责将视觉结果通过CAN卡发送出去
        Node(
            package='python_can_controller',
            executable='can_controller_node',
            name='python_can_controller_node',
            # 使用 output='screen' 可以在启动终端直接看到这个节点的日志输出
            output='screen' 
        ),

        # 5. 启动图像显示工具 (用于实时调试和展示)
        #    - 对应指令: ros2 run image_tools showimage --ros-args -r image:=/image_annotated
        Node(
            package='image_tools',
            executable='showimage',
            name='annotated_image_viewer',
            # 'remappings' 用于话题重映射，相当于命令行里的 -r image:=/image_annotated
            remappings=[
                ('image', '/image_annotated')
            ]
        )
    ])
